#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <chrono>

using namespace std
using namespace std::chrono_literals;

class CameraNode : public rclcpp::Node
{
public:
  CameraNode() : Node("camera_node")
  {
    // 声明参数
    this->declare_parameter("camera_id", 0);
    this->declare_parameter("frame_width", 640);
    this->declare_parameter("frame_height", 480);
    this->declare_parameter("fps", 30);
        
    cout<<"************************"<<endl;
    cout<<"************************"<<endl;
    cout<<"************************"<<endl;
    // 获取参数
    camera_id_ = this->get_parameter("camera_id").as_int();
    frame_width_ = this->get_parameter("frame_width").as_int();
    frame_height_ = this->get_parameter("frame_height").as_int();
    fps_ = this->get_parameter("fps").as_int();
    
    // 创建发布者 - 发布到video_det话题
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("video_det", 10);
    
    // 创建定时器
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(1000 / fps_),
      std::bind(&CameraNode::timer_callback, this)
    );
    
    // 初始化摄像头
    if (!init_camera()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize camera!");
      return;
    }
    
    RCLCPP_INFO(this->get_logger(), "Camera node started successfully!");
    RCLCPP_INFO(this->get_logger(), "Publishing to topic: video_det");
    RCLCPP_INFO(this->get_logger(), "Camera ID: %d, Resolution: %dx%d, FPS: %d", 
                camera_id_, frame_width_, frame_height_, fps_);
  }
  
  ~CameraNode()
  {
    if (cap_.isOpened()) {
      cap_.release();
    }
  }

private:
  bool init_camera()
  {
    cap_.open(camera_id_);
    if (!cap_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(), "Cannot open camera %d", camera_id_);
      return false;
    }
    
    // 设置摄像头参数
    cap_.set(cv::CAP_PROP_FRAME_WIDTH, frame_width_);
    cap_.set(cv::CAP_PROP_FRAME_HEIGHT, frame_height_);
    cap_.set(cv::CAP_PROP_FPS, fps_);
    
    // 验证设置是否成功
    double actual_width = cap_.get(cv::CAP_PROP_FRAME_WIDTH);
    double actual_height = cap_.get(cv::CAP_PROP_FRAME_HEIGHT);
    double actual_fps = cap_.get(cv::CAP_PROP_FPS);
    
    RCLCPP_INFO(this->get_logger(), "Camera initialized with:");
    RCLCPP_INFO(this->get_logger(), "  Width: %d (actual: %.0f)", frame_width_, actual_width);
    RCLCPP_INFO(this->get_logger(), "  Height: %d (actual: %.0f)", frame_height_, actual_height);
    RCLCPP_INFO(this->get_logger(), "  FPS: %d (actual: %.0f)", fps_, actual_fps);
    return true;
  }
  
  void timer_callback()
  {
    if (!cap_.isOpened()) {
      RCLCPP_ERROR(this->get_logger(), "Camera is not opened!");
      return;
    }
    
    cv::Mat frame;
    cap_ >> frame;
    
    if (frame.empty()) {
      RCLCPP_WARN(this->get_logger(), "Failed to capture frame from camera");
      return;
    }
    
    // 转换图像格式并发布
    try {
      auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg();
      msg->header.stamp = this->now();
      msg->header.frame_id = "camera_frame";
      
      image_pub_->publish(*msg);
      
      // 显示帧率信息
      static int frame_count = 0;
      static auto last_time = this->now();
      frame_count++;
      
      if (frame_count % 30 == 0) {
        auto current_time = this->now();
        auto duration = (current_time - last_time).seconds();
        double actual_fps = 30.0 / duration;
        RCLCPP_INFO(this->get_logger(), "Publishing frame %d, FPS: %.1f", frame_count, actual_fps);
        last_time = current_time;
      }
      
    } catch (cv_bridge::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
    }
  }
  
  cv::VideoCapture cap_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  
  int camera_id_;
  int frame_width_;
  int frame_height_;
  int fps_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<CameraNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  std::count<<"**********************"<<std::endl;
  std::count<<"**********************"<<std::endl;
  std::count<<"**********************"<<std::endl;
  std::count<<"**********************"<<std::endl;
  return 0;
}
